#include "Map.h"

Map::Map()
{
	// initialize all nodes to zero
	for (int i = 0; i < gGridSize; i++)
	{
		for (int j = 0; j < gGridSize; j++)
		{
			mNodeList[i][j] = 0;
		}
	}

	// let the child class define how the Nodes are populated
}

Map::~Map()
{
	// delete anything the nodes are still pointing to
	for (int i = 0; i < gGridSize; i++)
	{
		for (int j = 0; j < gGridSize; j++)
		{
			if (mNodeList[i][j] != 0)
			{
				delete mNodeList[i][j];
			}
		}
	}
}


std::vector<Node*> Map::FindPath(Coordinate start, Coordinate goal)
{
	

	// function A*(start,goal)
	// closedset := the empty set    // The set of nodes already evaluated.
	std::vector<Node*> closedset;

	// openset := {start}    // The set of tentative nodes to be evaluated, initially containing the start node
	std::vector<Node*> openset;
	openset.push_back(mNodeList[start.x][start.y]);

	// came_from := the empty map    // The map of navigated nodes.
	Node* came_from[gGridSize][gGridSize];

	if (GetNode(goal) == 0)
	{
		std::vector<Node*> empty;
		return empty;
	}

	// g_score[start] := 0    // Cost from start along best known path.
	float g_score[gGridSize][gGridSize];
	g_score[start.x][start.y] = 0.0f;

	// Estimated total cost from start to goal through y.
	float f_score[gGridSize][gGridSize];

	// f_score[start] := g_score[start] + heuristic_cost_estimate(start, goal)
	f_score[start.x][start.y] = g_score[start.x][start.y] + HeuristicCostEstimate(start, goal);

	// while openset is not empty
	while(openset.size() > 0)
	{
		// current := the node in openset having the lowest f_score[] value
		Node* current = 0;
		float lowestFScore = 999999.0f;
		std::vector<Node*>::iterator i, keepIterator;
		for(i = openset.begin(); i != openset.end(); i++)
		{
			float score = f_score[(*i)->mCoord.x][(*i)->mCoord.y];
			if (score < lowestFScore)
			{
				keepIterator = i; // store the location of this object
				current = (*i);
				lowestFScore = score;
			}
		}

		// if current = goal
		if (current == mNodeList[goal.x][goal.y])
		{
			// return reconstruct_path(came_from, goal)
			return ReconstructPath(came_from, mNodeList[start.x][start.y], mNodeList[goal.x][goal.y]);
		}
		
		// remove current from openset
		openset.erase(keepIterator);

		// add current to closedset
		closedset.push_back(current);

		// for each neighbor in neighbor_nodes(current)
		for(int i = 0; i < dir; i++)
		{
			bool skip = false;

			// figure out this neighbor's coordinates
			Coordinate neighbor;
			neighbor.x = current->mCoord.x + dx[i];
			neighbor.y = current->mCoord.y + dy[i];

			// determine if this neighbor is valid
			if (neighbor.x < 0 || neighbor.y < 0 || neighbor.x > gGridSize-1 || neighbor.y > gGridSize-1)
			{
				skip = true;
			}

			// check if the neighbor is not in closedset
			if (!skip)
			{
				for (std::vector<Node*>::iterator j = closedset.begin(); j != closedset.end(); ++j)
				{
					if(((*j)->mCoord.x == neighbor.x) && ((*j)->mCoord.y == neighbor.y))
					{
						// continue
						skip = true;
						break;
					}
				}
			}

			if (!skip)
			{
				//  tentative_g_score := g_score[current] + dist_between(current,neighbor)
				float tentative_g_score = g_score[current->mCoord.x][current->mCoord.y] + HeuristicCostEstimate(current->mCoord, neighbor); // reuse the heuristic cost since its basically the distance formula
				
				
				Node* neighborNode = mNodeList[neighbor.x][neighbor.y];

				bool found = false;
				for (std::vector<Node*>::iterator j = openset.begin(); j != openset.end(); ++j)
				{
					if ((*j) == neighborNode)
					{
						found = true;
					}
				}

				// if neighbor not in openset or tentative_g_score < g_score[neighbor]
				if (!found || tentative_g_score < g_score[neighbor.x][neighbor.y])
				{
					// add neighbor to openset
					openset.push_back(neighborNode);		
					// came_from[neighbor] := current
					came_from[neighbor.x][neighbor.y] = current;
					// g_score[neighbor] := tentative_g_score
					g_score[neighbor.x][neighbor.y] = tentative_g_score;
					// f_score[neighbor] := g_score[neighbor] + heuristic_cost_estimate(neighbor, goal)
					f_score[neighbor.x][neighbor.y] = g_score[neighbor.x][neighbor.y] + HeuristicCostEstimate(neighbor, goal);
				}
			}
		}
	}

	// return failure

}

float Map::HeuristicCostEstimate(Coordinate start, Coordinate goal)
{
	float x2 = (start.x - goal.x) * (start.x - goal.x);
	float y2 = (start.y - goal.y) * (start.y - goal.y);

	return sqrt(x2 + y2);
}

std::vector<Node*> Map::ReconstructPath(Node* came_from[gGridSize][gGridSize], Node* start, Node* goal)
{
	//function reconstruct_path(came_from, current_node)
	// if came_from[current_node] is set
		// p := reconstruct_path(came_from, came_from[current_node])1
			// return (p + current_node)
	// else
		// return current_node
	std::vector<Node*> path;

	Node* current = goal;

	while(current != start)
	{
		path.insert(path.begin(), current);
		current = came_from[current->mCoord.x][current->mCoord.y];
	}

	path.insert(path.begin(), start);

	return path;
}

Node* Map::GetNode(Coordinate coord)
{
	return GetNode(coord.x, coord.y);
}

Node* Map::GetNode(int x, int y)
{
	if (x < 0 || x >= gGridSize || y < 0 || y >= gGridSize)
	{
		// incorrect node position
		return 0;
	}
	else
	{
		return mNodeList[x][y];
	}
}